Skip to content

feat(imu,gnss): IMU lever arm centripetal, GPS pre-heading option, TF auto-resolve#17

Open
cedbossneo wants to merge 1 commit intomanankharwar:mainfrom
cedbossneo:pr/feat-imu-lever-arm-and-gps-pre-heading
Open

feat(imu,gnss): IMU lever arm centripetal, GPS pre-heading option, TF auto-resolve#17
cedbossneo wants to merge 1 commit intomanankharwar:mainfrom
cedbossneo:pr/feat-imu-lever-arm-and-gps-pre-heading

Conversation

@cedbossneo
Copy link
Copy Markdown
Contributor

Two complementary additions for frame-aware sensor fusion on robots with
sensors offset from base_link, plus a ROS-wrapper improvement that lets
the URDF be the single source of truth for the geometry.

  1. IMU lever arm compensation (core)
    The IMU rarely sits at the base_link origin. At offset r, the
    accelerometer reads
    a_imu = a_base + ω×(ω×r) + α×r + g_body
    Previously we dropped everything except a_base. With an IMU 30 cm
    from base_link on a robot spinning at 0.5-1.0 rad/s, the centripetal
    term contributes ~0.09-0.35 m/s² of apparent horizontal acceleration
    that the filter attributed (wrongly) to real motion or bias drift.

    • New sensors::ImuLeverArm struct and sensors::ImuParams::lever_arm.
    • New imu_measurement_function_with_lever_arm(...) factory returns
      a lambda that adds ω×(ω×r) = (ω·r)ω − (ω·ω)r to the predicted
      accel. The α×r tangential term is dropped (α not in state) and
      remains absorbed by accel_noise; negligible under typical
      ground-robot control bandwidth.
    • fusioncore.cpp selects the lever-arm-aware h() when the configured
      arm is non-zero, both for live update_imu() and the retrodiction
      replay path — so the compensation stays consistent when a delayed
      sensor forces the filter to re-apply buffered IMU frames.
    • New runtime setter FusionCore::set_imu_lever_arm() so the ROS
      wrapper can populate the arm from TF after construction without
      re-instantiating the filter.
  2. GNSS "apply lever arm pre-heading" option (core)
    Default behaviour: the antenna lever arm only takes effect AFTER
    heading_validated_ flips true (dock compass / dual-GNSS / 5 m of
    straight driving). That gate exists because the initial yaw may be
    way off, and a wrong-yaw lever arm projection can feed spurious
    position innovations into the filter.

    • New GnssParams::apply_lever_arm_pre_heading (default false,
      preserving existing behaviour).
    • When enabled, the lever arm is applied from the first fix, turning
      every GPS observation into an active yaw correction via the
      position-orientation coupling in the measurement Jacobian. Safe
      with Mahalanobis gating on and especially useful with RTK-grade
      fixes: converges yaw in a few fixes instead of waiting for the
      straight-line accumulator.
  3. ROS wrapper: auto-resolve lever arms from TF (fusion_node.cpp)
    Addresses "why doesn't FusionCore just adapt to URDF?". The core
    library stays framework-agnostic, but the ROS wrapper now:

    • Declares new params imu.lever_arm_x/y/z and
      gnss.apply_lever_arm_pre_heading.
    • On the first IMU message: if the user left the lever-arm params
      at zero, looks up base_frame -> imu_frame via tf2 and copies the
      translation into FusionCore's config via set_imu_lever_arm(). The
      rotation is already applied by the existing frame-rotation logic.
      A non-zero param acts as an explicit override.
    • On the first GPS fix: same treatment for the primary antenna,
      using msg->header.frame_id (typically "gnss_link" or "gps") as
      the lookup target.
    • TF lookup failures are logged (throttled) and the lever arm is
      left at zero — so missing-URDF installs still run, just without
      the centripetal/offset correction.

Together these close the observed gap between the filter's
position-velocity model and the physical geometry described by the
URDF, without requiring the user to maintain two copies of the same
numbers.

… auto-resolve

Two complementary additions for frame-aware sensor fusion on robots with
sensors offset from base_link, plus a ROS-wrapper improvement that lets
the URDF be the single source of truth for the geometry.

1. IMU lever arm compensation (core)
   The IMU rarely sits at the base_link origin. At offset r, the
   accelerometer reads
       a_imu = a_base + ω×(ω×r) + α×r + g_body
   Previously we dropped everything except a_base. With an IMU 30 cm
   from base_link on a robot spinning at 0.5-1.0 rad/s, the centripetal
   term contributes ~0.09-0.35 m/s² of apparent horizontal acceleration
   that the filter attributed (wrongly) to real motion or bias drift.

   - New sensors::ImuLeverArm struct and sensors::ImuParams::lever_arm.
   - New imu_measurement_function_with_lever_arm(...) factory returns
     a lambda that adds ω×(ω×r) = (ω·r)ω − (ω·ω)r to the predicted
     accel. The α×r tangential term is dropped (α not in state) and
     remains absorbed by accel_noise; negligible under typical
     ground-robot control bandwidth.
   - fusioncore.cpp selects the lever-arm-aware h() when the configured
     arm is non-zero, both for live update_imu() and the retrodiction
     replay path — so the compensation stays consistent when a delayed
     sensor forces the filter to re-apply buffered IMU frames.
   - New runtime setter FusionCore::set_imu_lever_arm() so the ROS
     wrapper can populate the arm from TF after construction without
     re-instantiating the filter.

2. GNSS "apply lever arm pre-heading" option (core)
   Default behaviour: the antenna lever arm only takes effect AFTER
   heading_validated_ flips true (dock compass / dual-GNSS / 5 m of
   straight driving). That gate exists because the initial yaw may be
   way off, and a wrong-yaw lever arm projection can feed spurious
   position innovations into the filter.

   - New GnssParams::apply_lever_arm_pre_heading (default false,
     preserving existing behaviour).
   - When enabled, the lever arm is applied from the first fix, turning
     every GPS observation into an active yaw correction via the
     position-orientation coupling in the measurement Jacobian. Safe
     with Mahalanobis gating on and especially useful with RTK-grade
     fixes: converges yaw in a few fixes instead of waiting for the
     straight-line accumulator.

3. ROS wrapper: auto-resolve lever arms from TF (fusion_node.cpp)
   Addresses "why doesn't FusionCore just adapt to URDF?". The core
   library stays framework-agnostic, but the ROS wrapper now:
   - Declares new params imu.lever_arm_x/y/z and
     gnss.apply_lever_arm_pre_heading.
   - On the first IMU message: if the user left the lever-arm params
     at zero, looks up base_frame -> imu_frame via tf2 and copies the
     translation into FusionCore's config via set_imu_lever_arm(). The
     rotation is already applied by the existing frame-rotation logic.
     A non-zero param acts as an explicit override.
   - On the first GPS fix: same treatment for the primary antenna,
     using msg->header.frame_id (typically "gnss_link" or "gps") as
     the lookup target.
   - TF lookup failures are logged (throttled) and the lever arm is
     left at zero — so missing-URDF installs still run, just without
     the centripetal/offset correction.

Together these close the observed gap between the filter's
position-velocity model and the physical geometry described by the
URDF, without requiring the user to maintain two copies of the same
numbers.
@manankharwar
Copy link
Copy Markdown
Owner

Really solid PR....

Two things to fix before we merge:

1. Stale comment in fusion_node.cpp
The comment references fc_->update_config_imu_lever_arm() but the actual function called is fc_->set_imu_lever_arm(). Looks like the function was renamed at some point but the comment wasn't updated.

2. fusioncore.yaml not updated
Four new parameters are declared in the code but missing from fusioncore_ros/config/fusioncore.yaml:

  • imu.lever_arm_x: 0.0
  • imu.lever_arm_y: 0.0
  • imu.lever_arm_z: 0.0
  • gnss.apply_lever_arm_pre_heading: false

The YAML is the single source of truth for users.... if a parameter isn't there, users won't know it exists. Please add all four with descriptions similar to the existing lever arm params.

Once those two are addressed, happy to merge!

@manankharwar
Copy link
Copy Markdown
Owner

Hey, just checking in on this one.... the two fixes are really small, and the underlying work is solid. If you get a chance to push those changes, I'm ready to merge.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants